

using System.Globalization;
using System;
using EarthGeospatial;
using Unity.Mathematics;

public class GeoTransforms
{
    private Ellipsoid _ellipsoid;
    private double3 _center;

    double4x4 _georeferencedToEcef;
    double4x4 _ecefToGeoreferenced;
    double4x4 _ueAbsToEcef;
    double4x4 _ecefToUeAbs;
    public GeoTransforms() : this(Ellipsoid.WGS84, double3.zero)
    {

    }
    public GeoTransforms(Ellipsoid ellipsoid, double3 center)
    {
        this._ellipsoid = ellipsoid;
        this._center = center;

        _georeferencedToEcef = double4x4.identity;
        _ecefToGeoreferenced = double4x4.identity;
        _ueAbsToEcef = double4x4.identity;
        _ecefToUeAbs = double4x4.identity;

        updateTransforms();
    }

    void setCenter(double3 center)
    {
        bool3 eq = this._center != center;
        if (eq.x && eq.y && eq.z)
        {
            this._center = center;
            updateTransforms();
        }
    }
    void setEllipsoid(Ellipsoid ellipsoid)
    {
        bool3 eq = this._ellipsoid.getRadii() != ellipsoid.getRadii();
        if (eq.x && eq.y && eq.z)
        {
            this._ellipsoid = ellipsoid;
            updateTransforms();
        }
    }

    void updateTransforms()
    {
        this._georeferencedToEcef = Transforms.eastNorthUpToFixedFrame(_center, _ellipsoid);
        this._ecefToGeoreferenced = math.inverse(this._georeferencedToEcef);

        this._ueAbsToEcef = math.mul(math.mul(this._georeferencedToEcef, Earth3DTransforms.scaleToCesium), Earth3DTransforms.unrealToOrFromCesium);
        this._ecefToUeAbs = math.mul(math.mul(Earth3DTransforms.unrealToOrFromCesium, Earth3DTransforms.scaleToUnrealWorld), this._ecefToGeoreferenced);

        UnityEngine.Debug.Log(string.Format("GeoTransforms::updateTransforms with center {0} {1} {2} and ellipsoid radii {3} {4} {5}",
            _center.x,
            _center.y,
            _center.z,
            _ellipsoid.getRadii().x,
            _ellipsoid.getRadii().y,
            _ellipsoid.getRadii().z));
    }

    internal double3 TransformLongitudeLatitudeHeightToEcef(double3 longitudeLatitudeHeight)
    {
        var cartographic = Cartographic.fromDegrees(longitudeLatitudeHeight.x, longitudeLatitudeHeight.y, longitudeLatitudeHeight.z);
        return this._ellipsoid.cartographicToCartesian(cartographic);
    }

    internal Cartographic? TransformLongitudeEcefToLatitudeHeight(double3 cartesian)
    {
        return this._ellipsoid.cartesianToCartographic(cartesian);
    }

    internal double4x4 GetEllipsoidCenteredToAbsoluteUnrealWorldTransform()
    {
        return _ecefToUeAbs;
    }
}